// Copyright 2025 Enactic, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <linux/can.h>
#include <linux/can/raw.h>

#include <iostream>
#include <openarm/eyou_motor/eyou_motor_device_collection.hpp>

namespace openarm::eyou_motor {

EYOUDeviceCollection::EYOUDeviceCollection(canbus::CANSocket& can_socket)
    : can_socket_(can_socket),
      can_packet_encoder_(std::make_unique<CanPacketEncoder>()),
      can_packet_decoder_(std::make_unique<CanPacketDecoder>()),
      device_collection_(std::make_unique<canbus::CANDeviceCollection>(can_socket_)) {}

void EYOUDeviceCollection::enable_all() {
    for (auto eyou_device : get_eyou_devices()) {
        auto& motor = eyou_device->get_motor();
        CANPacket enable_packet = CanPacketEncoder::create_enable_command(motor);
        send_command_to_device(eyou_device, enable_packet);
    }
}

void EYOUDeviceCollection::disable_all() {
    for (auto eyou_device : get_eyou_devices()) {
        CANPacket disable_packet = CanPacketEncoder::create_disable_command(eyou_device->get_motor());
        send_command_to_device(eyou_device, disable_packet);
    }
}

void EYOUDeviceCollection::set_zero_all() {
    for (auto eyou_device : get_eyou_devices()) {
        CANPacket zero_packet = CanPacketEncoder::create_set_zero_command(eyou_device->get_motor());
        send_command_to_device(eyou_device, zero_packet);
    }
}

void EYOUDeviceCollection::refresh_one(int i) {
    auto eyou_device = get_eyou_devices()[i];
    auto& motor = eyou_device->get_motor();
    CANPacket refresh_packet = CanPacketEncoder::create_refresh_command(motor);
    send_command_to_device(eyou_device, refresh_packet);
}

void EYOUDeviceCollection::refresh_all() {
    for (auto eyou_device : get_eyou_devices()) {
        auto& motor = eyou_device->get_motor();
        CANPacket refresh_packet = CanPacketEncoder::create_refresh_command(motor);
        send_command_to_device(eyou_device, refresh_packet);
    }
}

void EYOUDeviceCollection::set_callback_mode_all(CallbackMode callback_mode) {
    for (auto eyou_device : get_eyou_devices()) {
        eyou_device->set_callback_mode(callback_mode);
    }
}

void EYOUDeviceCollection::query_param_one(int i, int RID) {
    CANPacket param_query =
        CanPacketEncoder::create_query_param_command(get_eyou_devices()[i]->get_motor(), RID);
    send_command_to_device(get_eyou_devices()[i], param_query);
}

void EYOUDeviceCollection::query_param_all(int RID) {
    for (auto eyou_device : get_eyou_devices()) {
        CANPacket param_query =
            CanPacketEncoder::create_query_param_command(eyou_device->get_motor(), RID);
        send_command_to_device(eyou_device, param_query);
    }
}

void EYOUDeviceCollection::send_command_to_device(std::shared_ptr<EYOUCANDevice> eyou_device,
                                                const CANPacket& packet) {
    if (can_socket_.is_canfd_enabled()) {
        canfd_frame frame = eyou_device->create_canfd_frame(packet.send_can_id, packet.data);
        can_socket_.write_canfd_frame(frame);
    } else {
        can_frame frame = eyou_device->create_can_frame(packet.send_can_id, packet.data);
        can_socket_.write_can_frame(frame);
    }
}

void EYOUDeviceCollection::mit_control_one(int i, const MITParam& mit_param) {
    CANPacket mit_cmd =
        CanPacketEncoder::create_mit_control_command(get_eyou_devices()[i]->get_motor(), mit_param);
    send_command_to_device(get_eyou_devices()[i], mit_cmd);
}

void EYOUDeviceCollection::mit_control_all(const std::vector<MITParam>& mit_params) {
    for (size_t i = 0; i < mit_params.size(); i++) {
        mit_control_one(i, mit_params[i]);
    }
}

std::vector<Motor> EYOUDeviceCollection::get_motors() const {
    std::vector<Motor> motors;
    for (auto eyou_device : get_eyou_devices()) {
        motors.push_back(eyou_device->get_motor());
    }
    return motors;
}

std::vector<std::shared_ptr<EYOUCANDevice>> EYOUDeviceCollection::get_eyou_devices() const {
    std::vector<std::shared_ptr<EYOUCANDevice>> eyou_devices;
    for (const auto& [id, device] : device_collection_->get_devices()) {
        auto eyou_device = std::dynamic_pointer_cast<EYOUCANDevice>(device);
        if (eyou_device) {
            eyou_devices.push_back(eyou_device);
        }
    }
    return eyou_devices;
}

}  // namespace openarm::eyou_motor
